use r2r::geometry_msgs::msg::{Twist, Vector3};
use r2r::{self, QosProfile};

use std::collections::HashMap;
use std::io::{stdout, Write};
use std::sync::{Arc, Mutex};
use std::time::{Duration, Instant};

use crossterm::{
    cursor::MoveTo,
    event::{self, Event, KeyCode, KeyModifiers},
    execute, queue,
    style::Print,
    terminal::{self, Clear, ClearType},
};

fn create_velocity_bindings() -> HashMap<char, Twist> {
    fn twist(linear_x: f64, linear_y: f64, linear_z: f64, angular_z: f64) -> Twist {
        Twist {
            linear: Vector3 {
                x: linear_x,
                y: linear_y,
                z: linear_z,
            },
            angular: Vector3 {
                x: 0.0,
                y: 0.0,
                z: angular_z,
            },
        }
    }

    HashMap::from([
        ('w', twist(1.0, 0.0, 0.0, 0.0)),  // 前进
        ('s', twist(-1.0, 0.0, 0.0, 0.0)), // 后退
        ('a', twist(0.0, 1.0, 0.0, 0.0)),  // 左移
        ('d', twist(0.0, -1.0, 0.0, 0.0)), // 右移
        ('q', twist(0.0, 0.0, 0.0, 1.0)),  // 左转
        ('e', twist(0.0, 0.0, 0.0, -1.0)), // 右转
    ])
}

fn print_instructions() {
    let lines = [
        "Reading from the keyboard and publishing to /cmd_vel",
        "---------------------------",
        "Movement keys:",
        "  q    w    e",
        "  a    s    d",
        "---------------------------",
        "CTRL-C to quit.",
    ];
    for line in lines {
        println!("{line}");
    }
    println!("\n\n\n\n"); // 分隔
}

#[tokio::main]
async fn main() -> Result<(), Box<dyn std::error::Error>> {
    let ctx = r2r::Context::create()?;
    let node = r2r::Node::create(ctx, "champ_teleop", "")?;
    let node_arc = Arc::new(Mutex::new(node));

    let node_clone = node_arc.clone();
    let velocity_publisher = node_clone
        .lock()
        .unwrap()
        .create_publisher::<Twist>("/cmd_vel", QosProfile::default())?;

    let speed = node_clone
        .lock()
        .unwrap()
        .get_parameter("speed")
        .unwrap_or(1.5);
    let turn = node_clone
        .lock()
        .unwrap()
        .get_parameter("turn")
        .unwrap_or(2.0);

    const INSTRUCTION_LINES: u16 = 13;
    const STATUS_LINE_START: u16 = INSTRUCTION_LINES + 4;

    print_instructions();

    terminal::enable_raw_mode()?;
    let mut stdout = stdout();

    let velocity_bindings = create_velocity_bindings();

    let is_key_pressed = Arc::new(Mutex::new(false));
    let last_key_time = Arc::new(Mutex::new(Instant::now()));

    let max_speed = speed;
    let max_turn = turn;
    let current_twist = Arc::new(Mutex::new(Twist::default()));
    let mut current_linear_speed = 0.0;
    let mut current_angular_speed = 0.0;

    loop {
        if event::poll(Duration::from_millis(5))? {
            if let Event::Key(key_event) = event::read()? {
                if key_event.modifiers.contains(KeyModifiers::CONTROL)
                    && matches!(key_event.code, KeyCode::Char('c'))
                {
                    break;
                }

                if let KeyCode::Char(c) = key_event.code {
                    let c = c.to_ascii_lowercase();
                    *is_key_pressed.lock().unwrap() = true;
                    *last_key_time.lock().unwrap() = Instant::now();

                    if let Some(base_twist) = velocity_bindings.get(&c) {
                        let mut twist = Twist::default();
                        if base_twist.linear.x != 0.0 {
                            twist.linear.x = if base_twist.linear.x > 0.0 {
                                current_linear_speed
                            } else {
                                -current_linear_speed
                            };
                        }
                        if base_twist.linear.y != 0.0 {
                            twist.linear.y = if base_twist.linear.y > 0.0 {
                                current_linear_speed
                            } else {
                                -current_linear_speed
                            };
                        }
                        if base_twist.angular.z != 0.0 {
                            twist.angular.z = if base_twist.angular.z > 0.0 {
                                current_angular_speed
                            } else {
                                -current_angular_speed
                            };
                        }

                        *current_twist.lock().unwrap() = twist.clone();

                        // 更新显示
                        queue!(
                            stdout,
                            MoveTo(0, STATUS_LINE_START),
                            Clear(ClearType::CurrentLine),
                            Print(format!(
                                "当前: 线速度 {:.2} m/s 角速度 {:.2} rad/s",
                                current_linear_speed, current_angular_speed
                            )),
                            MoveTo(0, STATUS_LINE_START + 1),
                            Clear(ClearType::CurrentLine),
                            Print(format!(
                                "线性 {:.2}, {:.2}, {:.2} | 角速度 {:.2}",
                                twist.linear.x, twist.linear.y, twist.linear.z, twist.angular.z
                            ))
                        )?;
                        stdout.flush()?;
                    } else {
                        match c {
                            'i' | 'l' => {
                                current_linear_speed = (current_linear_speed + 0.1).min(max_speed)
                            }
                            'k' | 'j' => {
                                current_linear_speed = (current_linear_speed - 0.1).max(0.0)
                            }
                            'u' => current_angular_speed = (current_angular_speed - 0.1).max(0.0),
                            'o' => {
                                current_angular_speed = (current_angular_speed + 0.1).min(max_turn)
                            }
                            _ => {
                                *current_twist.lock().unwrap() = Twist::default();
                                *is_key_pressed.lock().unwrap() = false;
                            }
                        }

                        let current_twist_guard = current_twist.lock().unwrap();
                        queue!(
                            stdout,
                            MoveTo(0, STATUS_LINE_START),
                            Clear(ClearType::CurrentLine),
                            Print(format!(
                                "当前: 线速度 {:.2} m/s 角速度 {:.2} rad/s",
                                current_linear_speed, current_angular_speed
                            )),
                            MoveTo(0, STATUS_LINE_START + 1),
                            Clear(ClearType::CurrentLine),
                            Print(format!(
                                "线性 {:.2}, {:.2}, {:.2} | 角速度 {:.2}",
                                current_twist_guard.linear.x,
                                current_twist_guard.linear.y,
                                current_twist_guard.linear.z,
                                current_twist_guard.angular.z
                            ))
                        )?;
                        stdout.flush()?;
                    }
                }
            }
        } else {
            let now = Instant::now();
            let last_time = *last_key_time.lock().unwrap();
            if now.duration_since(last_time) > Duration::from_millis(30) {
                let mut is_pressed = is_key_pressed.lock().unwrap();
                if *is_pressed {
                    *current_twist.lock().unwrap() = Twist::default();
                    *is_pressed = false;

                    queue!(
                        stdout,
                        MoveTo(0, STATUS_LINE_START),
                        Clear(ClearType::CurrentLine),
                        Print(format!(
                            "当前: 线速度 {:.2} m/s 角速度 {:.2} rad/s",
                            current_linear_speed, current_angular_speed
                        )),
                        MoveTo(0, STATUS_LINE_START + 1),
                        Clear(ClearType::CurrentLine),
                        Print("状态: 已停止")
                    )?;
                    stdout.flush()?;
                }
            }
        }

        // 发布速度指令
        {
            let current_twist_guard = current_twist.lock().unwrap();
            velocity_publisher
                .publish(&*current_twist_guard)
                .unwrap_or_else(|e| {
                    eprintln!("发布速度指令失败: {}", e);
                });
        }
    }

    // 清屏状态区域
    execute!(
        stdout,
        MoveTo(0, STATUS_LINE_START),
        Clear(ClearType::FromCursorDown)
    )?;

    // 禁用 raw mode 前发送停止命令（但打印移到后）
    let stop_twist = Twist::default();
    velocity_publisher.publish(&stop_twist).unwrap_or_else(|e| {
        eprintln!("发送停止命令失败: {}", e);
    });

    // 禁用 raw mode
    terminal::disable_raw_mode()?;

    // 现在在正常模式下打印退出消息，确保无缩进
    println!("\n收到 Ctrl-C，准备退出...");
    println!("发送停止命令...");
    println!("程序已退出，终端已恢复正常模式");

    Ok(())
}
